package frc.circuitrunners;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;

/**
 * @author James Liu
 * This class handles all Input and Output for the robot
 */
public class IO {
    
    /**
     * Boolean check for Initialized or not
     */
    public static boolean started = false;
    
    /**
     * Preset input numbers
     */
    private static final int JoyNum = 2, DINum = 1;
    
    /**
     * Global Input Identifiers
     * Self-explanatory, uses bitwise conditioning in IO code
     */
    public static final int JOY_DRIVE = 0;
    public static final int JOY_SHOOTER = 1;
    
    /**
     * Input sources
     */
    private static Joystick joysticks[];
    private static DigitalInput digitalInputs[];

    /**
     * Handlers for Camera and Dashboard
     */
    public static CameraHandler cameraHandler = new CameraHandler();
    public static DashboardHandler dashboardHandler = new DashboardHandler();
    
    /**
     * Initializes the class for use
     */
    public static void init(){
        if(started){
            return;
        }
        joysticks = new Joystick[JoyNum];
        digitalInputs = new DigitalInput[DINum];
        for(int i = 0; i < JoyNum; i++){
            joysticks[i] = new Joystick(i + 1);
        }
        for(int i = 0; i < DINum; i++){
            digitalInputs[i] = new DigitalInput(2, i + 1);
        }
        started = true;
    }

    public static boolean getJoystickButton(int joy, int button){
        return joysticks[joy].getRawButton(button);
    }
    
    public static double getJoystickAxis(int joy, int axis){
        return joysticks[joy].getRawAxis(axis);
    }
    
    public static double getJoystickX(int joy){
        return joysticks[joy].getX();
    }
    
    public static double getJoystickY(int joy){
        return joysticks[joy].getY();
    }
    
    public static double getJoystickZ(int joy){
        return joysticks[joy].getZ();
    }
    
    public static boolean getDigitalInput(int DI){
        return digitalInputs[DI].get();
    }
    
    /**
     * Handles ALL the image processing on the robot, primarily used for the Shooter
     */
    public static class CameraHandler implements Runnable {
        
        /**
         * Constants used by targetToLookFor for selective purposes
         */
        public static final int TOP = 0, LEFT = 1, RIGHT = 2, BOTTOM = 3;
        
        /**
         * Used to sort out which target to aim for. Must be assigned to TOP, LEFT, RIGHT, or BOTTOM, nothing else will be accepted
         */
        public static int targetToLookFor = TOP;
        
        public static double aimOffset;
        public static double rotationSpeed;
        /**
         * Automatically starts a thread to handle the image processing
         */
        public CameraHandler(){
            new Thread(this).start();
        }
        
        /**
         * Executed once when the thread starts
         */
        public synchronized void run() {
            Timer.delay(8.0);
            AxisCamera camera = AxisCamera.getInstance("10.10.2.11");
            System.out.println("Initialized!");
            camera.writeResolution(AxisCamera.ResolutionT.k320x240);
            int target;
            double oldError = 0.0, integral = 0.0, derivative = 0.0;
            final double targetX = 0.0, kP = 0.35, kI = 0, kD = 0;
            Servo cameraServo = new Servo(5);
            while(true){
                boolean top, bottom, left, right;
                top = IO.getJoystickButton(IO.JOY_SHOOTER, 2);
                bottom = IO.getJoystickButton(IO.JOY_SHOOTER, 2);
                left = IO.getJoystickButton(IO.JOY_SHOOTER, 4);
                right = IO.getJoystickButton(IO.JOY_SHOOTER, 5);
                if(top){
                    targetToLookFor = TOP;
                } else if(bottom){
                    targetToLookFor = BOTTOM;
                } else if(left){
                    targetToLookFor = LEFT;
                } else if(right){
                    targetToLookFor = RIGHT;
                }
                cameraServo.set(0.4);
                if(RobotSubsystem.STATE == RobotSubsystem.AUTOAIM){
                   try{
                       long startTime = System.currentTimeMillis();
                        ColorImage image = camera.getImage(); //Aquire Image for processing 
                        BinaryImage thresholdImage = image.thresholdRGB(0, 151, 172, 255, 132, 255); //Apply Color Threshold
                        BinaryImage convexHullImage = thresholdImage.convexHull(false);
                        BinaryImage filteredImage = convexHullImage.removeSmallObjects(true, 5);
                        ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports(); //Get Particle analysis report
                        System.out.println("Elapsed Time: " + (System.currentTimeMillis() - startTime));
                        System.out.println("Total Number of Targets: " + reports.length);
                        image.free(); //free memory of base image
                        thresholdImage.free(); //free memory of threoshold image
                        convexHullImage.free();
                        filteredImage.free();
                        if(reports.length > 0){
                            target = sort(reports); //Find wanted target
                            aimOffset = targetX - reports[target].center_mass_x_normalized; //Calculate error
                            derivative = oldError - aimOffset; //Calculate Derivative
                            integral += aimOffset; //calculate itnegral
                            oldError = aimOffset; //replaces oldError with the current error for derivative calculation
                            rotationSpeed = -limit(kP * aimOffset + kI * integral + kD * derivative);
                            System.out.println("Aim Offset: " + aimOffset);
                            System.out.println("Rotation Speed Set: " + rotationSpeed);
                        }
                    }catch(AxisCameraException ace){
                        ace.printStackTrace();
                    }catch(NIVisionException NIVe){
                        NIVe.printStackTrace();
                    }
                } else {
                    aimOffset = 0;
                    rotationSpeed = 0;
                    oldError = 0;
                    integral = 0;
                    derivative = 0;
                }
            }
        }
        
        /**
         * Limits the output to make sure that 
         * @param input
         * @return 
         */
        private double limit(double input){
            final double limit = 0.5;
            if(input >= limit){
                return limit;
            } else if(input <= -limit){
                return -limit;
            } else {
                return input;
            }
        }
        
        /**
         * Chooses which particle to aim toward
         * @param reports the particle analysis reports
         * @return the particle that is chosen to aim at
         */
        private int sort(ParticleAnalysisReport[] reports){
            int index = 0;
            switch(targetToLookFor){
                case BOTTOM:
                    int highestY = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_y_normalized > highestY){
                            index = i;
                        }
                    }
                    break;
                case LEFT:
                    int lowestX = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_x_normalized< lowestX){
                            index = i;
                        }
                    }
                    break;
                case RIGHT:
                    int highestX = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_x_normalized > highestX){
                            index = i;
                        }
                    }
                    break;
                case TOP:
                    int lowestY = 0;
                    for(int i = 0; i < reports.length; i++){
                        if(reports[i].center_mass_y_normalized < lowestY){
                            index = i;
                        }
                    }
                    break;
                default:
                    break;
            }
            return index;
        }
    }
}
